//
// File:	AIWisdom.cpp
//
// Purpose:	Test program for FFLL API
//
// Copyright � 2001 Louder Than A Bomb! Software
//
//
//

#include "Aria.h"
#include "ipc.h"


#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc_c.h"

#include <iostream>
#include <vector>
#include <stdio.h>

#include <cstdio>
#include <cstdlib>
#include <iostream>	// for i/o functions

#define TARGET	0 // target is 1st variable
#define OBSTACLES	1 // obstacles is 2nd variable

const int SONAR_NUM = 8;

const double PI = 3.1415926535897932384626433832795;

const double OBS_DIST_THRES = 1000; // mm

const double H_LIN_VEL = 30; // mm/s
const double N_LIN_VEL = 20;
const double L_LIN_VEL = 10;

const double H_ROT_VEL = PI; // rad/s
const double N_ROT_VEL = PI/2;
const double L_ROT_VEL = PI/4;

// const for steering module
const int DECEL_LROT = 0;
const int DECEL_RROT = 1;
const int DECEL_STRT = 2;
const int ACCEL_LROT = 3;
const int ACCEL_RROT = 4;
const int ACCEL_STRT = 5;
const double DEL_ANGULAR_VEL = PI/18;
const double DEL_LINEAR_VEL = 10;

double angular_vel = 0;
double linear_vel = 0;


using namespace cv;
using namespace std;

Mat target_stack;
vector<Point2f> target_stack_vect;


float getTargetPosition()
{
	char *centralhost="140.118.38.92";
	double pose[3];


	myIPC_Init(centralhost);
	Sleep(200);

	// get robot heading
	myGetRobotPose(pose);
	Sleep(200);
	printf("%f  %f  %f \n", pose[0], pose[1], pose[2]);

	// get delta angle
	// where's the function


	// do the minus

	myShutdown();	
	system("pause");

	return 1;
}

float getObstaclePosition()
{
	Mat rb_sonars = Mat::zeros(1, SONAR_NUM, CV_64F);

	// Check all sonars
	int val;
	for (int i=0; i<SONAR_NUM; i++)
	{	 
		val = getSonarRange(i);
		rb_sonars.at<double>(i) = val;

		// If sonar i < OBS_DIST_THRES, it's an obstacle
		// what if obstacle is big, blocking several sonars ??
		if (val < OBS_DIST_THRES)
		{
			flag = 1;
		}
	}

	// return value;
	return 1;	
}

void steering(int steering_mode, double steering_param)
{
	double K = steering_param;

	if (steering_mode == DECEL_LROT || steering_mode == DECEL_RROT || steering_mode == DECEL_STRT)
	{
		linear_vel = linear_vel - K*DEL_LINEAR_VEL;

		if (steering_mode != DECEL_STRT)
		{
			angular_vel = angular_vel - K*DEL_ANGULAR_VEL;
		}
	}
	else
		if (steering_mode == ACCEL_LROT || steering_mode == ACCEL_RROT || steering_mode == ACCEL_STRT)
		{
			linear_vel = linear_vel + K*DEL_LINEAR_VEL;

			if (steering_mode != DECEL_STRT)
			{
				angular_vel = angular_vel + K*DEL_ANGULAR_VEL;
			}
		}

	double V = linear_vel;
	double dtheta = angular_vel;

	double wr = (2*V+dtheta*L)/2/r;
	double wr = (2*V-dtheta*L)/2/r;

	robot.setVel2(wl*180/3.14,wr*180/3.14);

/*
	switch(steering_mode)  
	{
	case (DECEL_LROT):
		cout << "Run Away!";
		break;

	case (DECEL_RROT):
		cout << "Fight Defensively";
		break;

	case (DECEL_STRT):

		break;

	case (ACCEL_LROT):
		cout << "Run Away!";
		break;

	case (ACCEL_RROT):
		cout << "Fight Defensively";
		break;

	case (ACCEL_STRT):

		cout << "All Out Attack!";
		break;

	} // end switch
*/
}


void controller()
{

	// go straight to target
	

	// store a stack of targets


	// create new target if current target is blocked

}


int main(int argc, char* argv[])
{
	
	// Initialization
    float	target_pos, obstacles_pos; // values for input variables
    char	option;	// var for selection of what user wants to do
	FuzzyController FzCtrl(); 
	

	char c;

	//target_stack = Mat::zeros() -> use a  dynamic vector


	while (1)
	{

		// get obstacle status
		obstacles_pos = getObstaclePosition();

		// get target status
		target_pos = getTargetPosition();

		// get mode: virtual target OR real target
		
		
		// feed to fuzzy controller to have output decision 
		FuzzyController.getDecision(target_pos, obstacles_pos);

		// execute decision
		

		// press "Esc" to escape
		c = (char)waitKey(1);
		if (c == 27)
		{
			break;
		}
	}  

    return 0;

} // end main()


